from readAndroidData import readSqlite
from readAndroidData import AndroidPlot
from readAndroidData import KFattitudeAndroid
from readAndroidData import KFAndroidBias

delta_t = 0.02
def main():
    # plotAcce()
    plotPosi()

def plotAcce():
    Acce, Gyro, Line = readSqlite.readSqlite()

    getGroup = [[], [], [],
           [0, 0, 0],
           [0, 0, 0],
           [0, 0, 0],
           [1, 0, 0, 0],
                Acce[1], Acce[2], Acce[3], Gyro[1], Gyro[2], Gyro[3]]

    # KFattitudeAndroid.KFfilter(getGroup)

    AndroidPlot.plotAcce(Acce, Line)


def plotPosi():
    Acce, Gyro, Line = readSqlite.readSqlite()

    getGroup = [[], [], [],
           [0, 0, 0],
           [0, 0, 0],
           [0, 0, 0],
           [1, 0, 0, 0],
                Acce[1], Acce[2], Acce[3], Gyro[1], Gyro[2], Gyro[3]]

    Line_Px, Line_Py, Line_Pz = linearIntegrate(Line)

    # KFattitudeAndroid.KFfilter(getGroup)
    AndroidPlot.plotPosition(Line[0], Line_Px, Line_Py, Line_Pz, Acce, Gyro, Line)

    AndroidPlot.plotCovariance(Acce)


def linearIntegrate(Line):

    Line_X = Line[1]
    Line_Y = Line[2]
    Line_Z = Line[3]

    V_X0, V_Y0, V_Z0 = 0, 0, 0
    P_X0, P_Y0, P_Z0 = 0, 0, 0

    Line_Px, Line_Py, Line_Pz = [], [], []

    for i in range(0, len(Line_X)):
        V_X1 = V_X0 + delta_t * Line_X[i]
        V_Y1 = V_Y0 + delta_t * Line_Y[i]
        V_Z1 = V_Z0 + delta_t * Line_Z[i]
        P_X1 = P_X0 + V_X1 * delta_t + 0.5 * Line_X[i] * delta_t * delta_t
        P_Y1 = P_Y0 + V_Y1 * delta_t + 0.5 * Line_Y[i] * delta_t * delta_t
        P_Z1 = P_Z0 + V_Z1 * delta_t + 0.5 * Line_Z[i] * delta_t * delta_t

        V_X0, V_Y0, V_Z0 = V_X1, V_Y1, V_Z1
        P_X0, P_Y0, P_Z0 = P_X1, P_Y1, P_Z1

        Line_Px.append(P_X1)
        Line_Py.append(P_Y1)
        Line_Pz.append(P_Z1)

    return Line_Px, Line_Py, Line_Pz


if __name__ == '__main__':
    main()





